// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement.  The various license agreements may be found at
// the Magic Software web site.  This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf

#include "MgcRTLib.h"
#include "MgcMinimizeND.h"

//----------------------------------------------------------------------------
MgcMinimizeND::MgcMinimizeND (int iDimensions, Function oF,
    unsigned int uiMaxLevel, unsigned int uiMaxBracket,
    unsigned int uiMaxIterations, void* pvUserData)
    :
    m_kMinimizer(LineFunction,uiMaxLevel,uiMaxBracket)
{
    assert( iDimensions >= 1 && oF );

    m_iDimensions = iDimensions;
    m_oF = oF;
    m_uiMaxIterations = uiMaxIterations;
    m_pvUserData = pvUserData;

    m_afTCurr = new MgcReal[m_iDimensions];
    m_afTSave = new MgcReal[m_iDimensions];
    m_afDirectionStorage = new MgcReal[m_iDimensions*(m_iDimensions+1)];
    m_aafDirection = new MgcReal*[m_iDimensions+1];
    for (int i = 0; i <= m_iDimensions; i++)
        m_aafDirection[i] = &m_afDirectionStorage[i*m_iDimensions];
    m_afDConj = m_aafDirection[m_iDimensions];

    m_afLineArg = new MgcReal[m_iDimensions];
}
//----------------------------------------------------------------------------
MgcMinimizeND::~MgcMinimizeND ()
{
    delete[] m_afTCurr;
    delete[] m_afTSave;
    delete[] m_afDirectionStorage;
    delete[] m_aafDirection;
    delete[] m_afLineArg;
}
//----------------------------------------------------------------------------
void MgcMinimizeND::GetMinimum (const MgcReal* afT0, const MgcReal* afT1,
    const MgcReal* afTInitial, MgcReal* afTMin, MgcReal& rfFMin)
{
    // for 1D function callback
    m_kMinimizer.UserData() = this;

    // initial guess
    unsigned int uiQuantity = m_iDimensions*sizeof(MgcReal);
    m_fFCurr = m_oF(afTInitial,m_pvUserData);
    memcpy(m_afTSave,afTInitial,uiQuantity);
    memcpy(m_afTCurr,afTInitial,uiQuantity);

    // initialize direction set to the standard Euclidean basis
    memset(m_afDirectionStorage,0,uiQuantity*(m_iDimensions+1));
    int i, j;
    for (i = 0; i < m_iDimensions; i++)
        m_aafDirection[i][i] = 1.0;

    MgcReal fL0, fL1, fLMin;
    for (unsigned int uiI = 0; uiI < m_uiMaxIterations; uiI++)
    {
        // find minimum in each direction and update current location
        for (i = 0; i < m_iDimensions; i++)
        {
            m_afDCurr = m_aafDirection[i];
            ComputeDomain(afT0,afT1,fL0,fL1);
            m_kMinimizer.GetMinimum(fL0,fL1,0.0,fLMin,m_fFCurr);
            for (j = 0; j < m_iDimensions; j++)
                m_afTCurr[j] += fLMin*m_afDCurr[j];
        }

        // estimate a unit-length conjugate direction
        MgcReal fLength = 0.0;
        for (i = 0; i < m_iDimensions; i++)
        {
            m_afDConj[i] = m_afTCurr[i] - m_afTSave[i];
            fLength += m_afDConj[i]*m_afDConj[i];
        }

        const MgcReal fEpsilon = 1e-06;
        fLength = MgcMath::Sqrt(fLength);
        if ( fLength < fEpsilon )
        {
            // New position did not change significantly from old one.
            // Should there be a better convergence criterion here?
            break;
        }

        MgcReal fInvLength = 1.0/fLength;
        for (i = 0; i < m_iDimensions; i++)
            m_afDConj[i] *= fInvLength;

        // minimize in conjugate direction
        m_afDCurr = m_afDConj;
        ComputeDomain(afT0,afT1,fL0,fL1);
        m_kMinimizer.GetMinimum(fL0,fL1,0.0,fLMin,m_fFCurr);
        for (j = 0; j < m_iDimensions; j++)
            m_afTCurr[j] += fLMin*m_afDCurr[j];

        // cycle the directions and add conjugate direction to set
        m_afDConj = m_aafDirection[0];
        for (i = 0; i < m_iDimensions; i++)
            m_aafDirection[i] = m_aafDirection[i+1];

        // set parameters for next pass
        memcpy(m_afTSave,m_afTCurr,uiQuantity);
    }

    memcpy(afTMin,m_afTCurr,uiQuantity);
    rfFMin = m_fFCurr;
}
//----------------------------------------------------------------------------
void MgcMinimizeND::ComputeDomain (const MgcReal* afT0, const MgcReal* afT1,
    MgcReal& rfL0, MgcReal& rfL1)
{
    rfL0 = -MgcMath::INFINITY;
    rfL1 = +MgcMath::INFINITY;

    for (int i = 0; i < m_iDimensions; i++)
    {
        MgcReal fB0 = afT0[i] - m_afTCurr[i];
        MgcReal fB1 = afT1[i] - m_afTCurr[i];
        MgcReal fInv;
        if ( m_afDCurr[i] > 0 )
        {
            // valid t-interval is [B0,B1]
            fInv = 1.0/m_afDCurr[i];
            fB0 *= fInv;
            if ( fB0 > rfL0 )
                rfL0 = fB0;
            fB1 *= fInv;
            if ( fB1 < rfL1 )
                rfL1 = fB1;
        }
        else if ( m_afDCurr[i] < 0 )
        {
            // valid t-interval is [B1,B0]
            fInv = 1.0/m_afDCurr[i];
            fB0 *= fInv;
            if ( fB0 < rfL1 )
                rfL1 = fB0;
            fB1 *= fInv;
            if ( fB1 > rfL0 )
                rfL0 = fB1;
        }
    }

    // correction if numerical errors lead to values nearly zero
    if ( rfL0 > 0.0 )
        rfL0 = 0.0;
    if ( rfL1 < 0.0 )
        rfL1 = 0.0;
}
//----------------------------------------------------------------------------
MgcReal MgcMinimizeND::LineFunction (MgcReal fT, void* pvUserData)
{
    MgcMinimizeND* pkThis = (MgcMinimizeND*) pvUserData;

    for (int i = 0; i < pkThis->m_iDimensions; i++)
    {
        pkThis->m_afLineArg[i] = pkThis->m_afTCurr[i] +
            fT*pkThis->m_afDCurr[i];
    }

    MgcReal fResult = pkThis->m_oF(pkThis->m_afLineArg,pkThis->m_pvUserData);
    return fResult;
}
//----------------------------------------------------------------------------
